#!  /usr/bin/env python
# coding=utf-8

import rospy
import socket
import os
import json
import numpy as np
import math
import cv2 as cv
from serial_demo.msg import ti5_polyJC_Lists
from serial_demo.msg import ti5_motorStatus

from ti5_fkik import *


# L0 = 190
# L2 = 400
# L3 = 300
# LT = 300

# # MDH
# alfa_i_1 = np.zeros((6,1),np.float)
# a_i_1 = np.zeros((6,1),np.float)
# theta_i = np.zeros((6,1),np.float)
# d_i = np.zeros((6,1),np.float)

# pi = math.pi

# alfa_i_1[0]= 0 ;     a_i_1[0] = 0 ;    theta_i[0] = 0;       d_i[0] = 190

# alfa_i_1[1]= pi/2;   a_i_1[1] = 0;     theta_i[1] = pi/2;    d_i[1] = 0

# alfa_i_1[2]= 0;      a_i_1[2] = 400;   theta_i[2] = -pi/2;   d_i[2] = 0

# alfa_i_1[3]= -pi/2;  a_i_1[3] = 0;     theta_i[3] = 0;       d_i[3] = 300

# alfa_i_1[4]= pi/2;   a_i_1[4] = 0;     theta_i[4] = pi/2;    d_i[4] = 0

# alfa_i_1[5]= pi/2;   a_i_1[5] = 0;     theta_i[5] = 0;       d_i[5] = 300

# theta = np.zeros((8,6),np.float)

# Tw_00 = np.eye(4, dtype = np.float)
# Tw_00[2,3] = L0

# T6_t = np.eye(4, dtype = np.float)
# T6_t[2,3] = LT

# R00_6 = np.eye(3,dtype= np.float)

# class MDH_TF_f:
#     @staticmethod
#     def T_fram(alfa, a, theta, d):
#         # type:(np.ndarray, np.ndarray, np.ndarray, np.ndarray)->np.ndarray
#         T = np.zeros((6,4,4),dtype= np.float)
#         for i in range(0,4):
#             T[i,:,:] = np.eye(4, dtype=np.float)
#             pass
        
#         for i in range(0,6):
#             T[i,0,0] = math.cos(theta[i])
#             T[i,0,1] = -math.sin(theta[i])
#             T[i,0,2] = 0.0
#             T[i,0,3] = a[i]
#             T[i,1,0] = math.sin(theta[i]) * math.cos(alfa[i])
#             T[i,1,1] = math.cos(theta[i]) * math.cos(alfa[i])
#             T[i,1,2] = -math.sin(alfa[i])
#             T[i,1,3] = -math.sin(alfa[i]) * d[i]
#             T[i,2,0] = math.sin(theta[i]) * math.sin(alfa[i])
#             T[i,2,1] = math.cos(theta[i]) * math.sin(alfa[i])
#             T[i,2,2] = math.cos(alfa[i])
#             T[i,2,3] = math.cos(alfa[i]) * d[i]
#             pass
#         return T
#         pass

#     # 0 <= a < b <= n  Tf = array(n,4,4) has transfor of fram i-1 to i
#     @staticmethod
#     def T_fram_a_b(a,b,Tf):
#         # type:(int, int, np.ndarray)->np.ndarray
        
#         T = np.eye(4, dtype=np.float)
#         for i in range(0,b-a):
#             T = np.dot(T , Tf[a+i,:,:])
#             pass

#         return T
#         pass
    
#     @staticmethod
#     def R_fram_0_4(t1,t2,t3):
#         # type:(float,float,float)->np.ndarray
#         # R = np.eye(3,np.float)
#         # R[0,0] = math.cos(t1)*math.cos(t2)*math.cos(t3)*math.cos(t4) - math.cos(t1)*math.sin(t2)*math.sin(t3)*math.cos(t4) - math.sin(t1)*math.sin(t4)
#         # R[0,1] = -math.cos(t1)*math.cos(t2)*math.cos(t3)*math.sin(t4) + math.cos(t1)*math.cos(t2)*math.sin(t3)*math.sin(t4) - math.sin(t1)*math.cos(t4)
#         # R[0,2] = -math.cos(t1)*math.cos(t2)*math.sin(t3) - math.cos(t1)*math.sin(t2)*math.cos(t3)
#         # R[1,0] = math.sin(t1)*math.cos(t2)*math.cos(t3)*math.cos(t4) - math.sin(t1)*math.sin(t2)*math.sin(t3)*math.cos(t4) + math.cos(t1)*math.sin(t4)
#         # R[1,1] = -math.sin(t1)*math.cos(t2)*math.cos(t3)*math.sin(t4) + math.sin(t1)*math.sin(t2)*math.sin(t3)*math.sin(t4) + math.cos(t1)*math.cos(t4)
#         # R[1,2] = -math.sin(t1)*math.cos(t2)*math.sin(t3) - math.sin(t1)*math.sin(t2)*math.cos(t3)
#         # R[2,0] = math.sin(t2)*math.cos(t3)*math.cos(t4) + math.cos(t2)*math.sin(t3)*math.cos(t4)
#         # R[2,1] = -math.sin(t2)*math.cos(t3)*math.sin(t4) - math.cos(t2)*math.sin(t3)*math.sin(t4)
#         # R[2,2] = -math.sin(t2)*math.sin(t3) + math.cos(t2)*math.cos(t3)

#         # optimize
#         R = np.eye(3,dtype= np.float)
#         R[0,0] = math.cos(t1)*math.cos(t2)*math.cos(t3) - math.cos(t1)*math.sin(t2)*math.sin(t3)

#         R[0,1] = -math.sin(t1)

#         R[0,2] = -math.cos(t1)*math.cos(t2)*math.sin(t3) - math.cos(t1)*math.sin(t2)*math.cos(t3)

#         R[1,0] = math.sin(t1)*math.cos(t2)*math.cos(t3) - math.sin(t1)*math.sin(t2)*math.sin(t3)

#         R[1,1] = math.cos(t1)

#         R[1,2] = -math.sin(t1)*math.cos(t2)*math.sin(t3) - math.sin(t1)*math.sin(t2)*math.cos(t3)

#         R[2,0] = math.sin(t2)*math.cos(t3) + math.cos(t2)*math.sin(t3)

#         R[2,1] = 0

#         R[2,2] = -math.sin(t2)*math.sin(t3) + math.cos(t2)*math.cos(t3)


#         return R
#         pass

#     @staticmethod
#     def R_fram_0_4_inv(t1,t2,t3):
#         # type:(float,float,float)->np.ndarray
        
#         # optimize


#         Rstar = np.eye(3,dtype= np.float) # R*
#         Rstar[0,0] = math.cos(t1)*math.cos(t2+t3)
#         Rstar[0,1] = math.sin(t1)*math.cos(t2+t3)
#         Rstar[0,2] = math.sin(t2+t3)
#         Rstar[1,0] = -math.sin(t1)
#         Rstar[1,1] = math.cos(t1)
#         Rstar[1,2] = 0
#         Rstar[2,0] = -math.cos(t1)*math.sin(t2+t3) 
#         Rstar[2,1] = -math.sin(t1)*math.sin(t2+t3)
#         Rstar[2,2] = math.cos(t2+t3)

#         # invR=R*/|R|  |rotM|==1

#         return Rstar
#         pass

#     pass

# def mdhTfM(alfa_i_1, a_i_1, theta_i, d_i):
#     # type:(float, float, float, float)->np.ndarray
#     T = np.eye(4, dtype=np.float)
#     T[0,0] = math.cos(theta_i)
#     T[0,1] = -math.sin(theta_i)
#     T[0,2] = 0
#     T[0,3] = a_i_1
#     T[1,0] = math.sin(theta_i)*math.cos(alfa_i_1)
#     T[1,1] = math.cos(theta_i)*math.cos(alfa_i_1)
#     T[1,2] = -math.sin(alfa_i_1)
#     T[1,3] = -math.sin(alfa_i_1)*d_i
#     T[2,0] = math.sin(theta_i)*math.sin(alfa_i_1)
#     T[2,1] = math.cos(theta_i)*math.sin(alfa_i_1)
#     T[2,2] = math.cos(alfa_i_1)
#     T[2,3] = math.cos(alfa_i_1)*d_i
 
#     return T
#     pass

# def ti5FK(theta):
#     # type:(np.ndarray)->np.ndarray
#     T01 = mdhTfM(0.0,     0.0,   0+theta[0],     190.0)
#     T12 = mdhTfM(pi/2,    0.0,   pi/2+theta[1],  0.0  )
#     T23 = mdhTfM(0.0,     400.0, -pi/2+theta[2], 0.0  )
#     T34 = mdhTfM(-pi/2,   0.0,   0+theta[3],     300.0)
#     T45 = mdhTfM(pi/2,    0.0,   pi/2+theta[4],  0.0  )
#     T56 = mdhTfM(pi/2,    0.0,   0+theta[5],     300.0)
#     Tf = np.dot(np.dot(np.dot(np.dot(np.dot(T01, T12), T23), T34), T45), T56)

#     return Tf
#     pass

# def ti5_ik(cJdeg, Targ):
#     # type:(np.ndarray,np.ndarray)->np.ndarray
#     """ti5逆解脚本
#         input:current joint
#               target pose
#         output: solution
#     """
#     theta = np.zeros((8,6),np.float)
#     theta16 = np.zeros((16,6),np.float)
#     px = Targ[0,3]-Targ[0,2]*LT
#     py = Targ[1,3]-Targ[1,2]*LT
#     pz = Targ[2,3]-Targ[2,2]*LT - L0

#     R00_6[:] = Targ[0:3,0:3]
    
#     extraSolution = 0

#     # theta1 1 solution
#     if abs(px) <= 0.0001 and abs(py) <= 0.0001: # x,y=0,0
#         theta1_1 = 0
#     else:
#         theta1_1 = math.atan2(py,px)
#         pass

#     # theta1 2 solution
#     if theta1_1>0:
#         theta1_2 = theta1_1 - pi
#     else:
#         theta1_2 = theta1_1 + pi  
#         pass
    
#     # theta2,3 
#     xx = math.sqrt(px*px + py*py)
    
#     LL = px*px + py*py + pz*pz
#     L = math.sqrt(LL)

#     fa = math.atan2(abs(pz),xx)

#     if L==L2+L3:
#         # only one solution
#         if pz>=0:
#             theta2_1 = -pi/2+fa
#             theta3_1 = 0
#             theta2_2 = -pi/2+fa
#             theta3_2 = 0     
#         elif pz<0:
#             theta2_1 = -pi/2-fa
#             theta3_1 = 0
#             theta2_2 = -pi/2-fa
#             theta3_2 = 0      
#     else:
#         alfa = math.acos((LL + L2*L2 - L3*L3)/(2*L*L2))
#         beta = math.acos((L2*L2 + L3*L3 - LL)/(2*L2*L3))
#         if pz>=0:
#             # 1 solution
#             theta2_1 = alfa+fa-pi/2
#             theta3_1 = beta-pi
#             # 2 solution
#             theta2_2 = -alfa+fa-pi/2
#             theta3_2 = -beta+pi
#         elif pz<0:
#             # 1 solution
#             theta2_1 = alfa-fa-pi/2
#             theta3_1 = beta-pi
#             # 2 solution
#             theta2_2 = -alfa-fa-pi/2
#             theta3_2 = -beta+pi
#         pass

#     # front 3 axes
#     # 1
#     theta[0,0] = theta1_1
#     theta[0,1] = theta2_1
#     theta[0,2] = theta3_1

#     theta[4,0] = theta1_1
#     theta[4,1] = theta2_1
#     theta[4,2] = theta3_1

#     # 2
#     theta[1,0] = theta1_2
#     theta[1,1] = -theta2_1
#     theta[1,2] = -theta3_1

#     theta[5,0] = theta1_2
#     theta[5,1] = -theta2_1
#     theta[5,2] = -theta3_1

#     # 3
#     theta[2,0] = theta1_1
#     theta[2,1] = theta2_2
#     theta[2,2] = theta3_2

#     theta[6,0] = theta1_1
#     theta[6,1] = theta2_2
#     theta[6,2] = theta3_2

#     # 4
#     theta[3,0] = theta1_2
#     theta[3,1] = -theta2_2
#     theta[3,2] = -theta3_2

#     theta[7,0] = theta1_2
#     theta[7,1] = -theta2_2
#     theta[7,2] = -theta3_2

#     # theta4,5,6******************

#     # solve last 3 axes by front 3 axes
#     for j in range(0,4):
#         th = np.zeros((3,1),np.float)
      
#         th = theta[j,0:3] 
           
#         R00_4_inv = MDH_TF_f.R_fram_0_4_inv(th[0],th[1],th[2])
        
#         R4_6 = np.dot(R00_4_inv, R00_6)  # R4_6=(R00_4^-1)*R00_6
#         R4_6_33 = R4_6[2,2]

#         if (abs(R4_6_33-1)>0.000000001) and ( abs(R4_6_33+1)>0.000000001): # !=1 and !=-1
#             beta = math.atan2(math.sqrt(R4_6[2,0]*R4_6[2,0] + R4_6[2,1]*R4_6[2,1]), R4_6_33)
#             alfa = math.atan2(R4_6[1,2]/math.sin(beta), R4_6[0,2]/math.sin(beta))
#             gama = math.atan2(R4_6[2,1]/math.sin(beta), -R4_6[2,0]/math.sin(beta))
#         elif abs(R4_6_33-1)<=0.000000001: # =1
#             alfa = 0
#             beta = 0
#             gama = math.atan2(-R4_6[0,1],R4_6[0,0])
#         elif abs(R4_6_33+1)<=0.000000001: # =-1
#             alfa = 0
#             beta = pi
#             gama = math.atan2(R4_6[0,1],-R4_6[0,0])
#             pass

#         # 1 solution
#         if alfa<0:
#             theta4_1 = alfa+pi
#         else:
#             theta4_1 = alfa-pi
#             pass
        
#         theta5_1 = beta+pi/2 # add axis 5 default extra offset
#         theta6_1 = gama 

#         # 2 solution
#         theta5_2 = -beta+pi/2

#         if theta4_1>=0:
#             theta4_2 = theta4_1-pi
#         else:
#             theta4_2 = theta4_1+pi   
#             pass

#         if theta6_1>=0:
#             theta6_2 = theta6_1-pi
#         else:
#             theta6_2 = theta6_1+pi   
#             pass
        
#         # each set of solution for the front 3 axes 
#         # corresponds to 2 sets of solution for the last 3 axes
#         theta[j,3] = theta4_1
#         theta[j,4] = theta5_1
#         theta[j,5] = theta6_1
        
#         theta[j+4,3] = theta4_2
#         theta[j+4,4] = theta5_2
#         theta[j+4,5] = theta6_2

#         if (pi-abs(theta6_1)) <= 0.001: # theta6_1= +-180
#             extraSolution = extraSolution+1

#             theta16[7+extraSolution,0] = theta[j,0]
#             theta16[7+extraSolution,1] = theta[j,1]
#             theta16[7+extraSolution,2] = theta[j,2]
#             theta16[7+extraSolution,3] = theta[j,3]
#             theta16[7+extraSolution,4] = theta[j,4]
#             theta16[7+extraSolution,5] = -theta6_1
#         elif abs(theta6_1) <= 0.001: # theta6_1=0 theat6_2= +-180
#             extraSolution = extraSolution+1

#             theta16[7+extraSolution,0] = theta[j+4,0]
#             theta16[7+extraSolution,1] = theta[j+4,1]
#             theta16[7+extraSolution,2] = theta[j+4,2]
#             theta16[7+extraSolution,3] = theta[j+4,3]
#             theta16[7+extraSolution,4] = theta[j+4,4]
#             theta16[7+extraSolution,5] = -theta6_2
            
#             pass

#         pass # end of for j in range(0,4):
#     theta16[0:8,:] = theta[0:8,:]
#     if extraSolution < 8:
#         theta16 = np.delete(theta16,obj=np.s_[8+extraSolution:16], axis=0) 
#         pass
    
#     print(np.around(theta16*180/pi,3))
 
#     return theta16
#     pass

# def ti5_optIK(cJdeg, Targ):
#     # type:(np.ndarray,np.ndarray)->np.ndarray
#     try:
#         theta = ti5_ik(cJdeg, Targ)
#     except Exception as e:
#         print('error occurred')
#         print(e)
#         theta = np.array(cJdeg, np.float)


#     numOfSolution = np.size(theta)/6
#     theta = theta*(180/pi)
    
#     # select solutions by joint limit
#     # joint limit : axes 1  4  are [-170,170] 
#     #               axis2 is [-90,90]   axis5 is [-50,200]
#     #               axis3 is [-150,150] axis6 is [-180,180]

    

#     # select axis2******************
#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
        
#     #     minusTheta2 = (theta[:,1] - 90)
#     #     minusTheta2maxJ = np.max(minusTheta2)
#     #     maxJ2index = np.argmax(minusTheta2)
#     #     if(0 >= minusTheta2maxJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=maxJ2index, axis=0)  
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     addTheta2 = (theta[:,1] + 90)
#     #     addTheta2minJ = np.min(addTheta2)
#     #     minJ2index = np.argmin(addTheta2)
#     #     if(0 <= addTheta2minJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=minJ2index, axis=0)  
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # # select axis5**************
#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     minusTheta5 = (theta[:,4] - 200)
#     #     minusTheta5maxJ = np.max(minusTheta5)
#     #     maxJ5index = np.argmax(minusTheta5)
#     #     if(0 > minusTheta5maxJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=maxJ5index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     addTheta5 = (theta[:,4] + 50)
#     #     addTheta5minJ = np.min(addTheta5)
#     #     minJ5index = np.argmin(addTheta5)
#     #     if(0 <= addTheta5minJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=minJ5index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass
        
#     # # select axis 1 4**************
#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     minusTheta1 = (theta[:,0] - 170)
#     #     minusTheta1maxJ = np.max(minusTheta1)
#     #     maxJ1index = np.argmax(minusTheta1)
#     #     if(0 > minusTheta1maxJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=maxJ1index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     addTheta1 = (theta[:,0] + 170)
#     #     addTheta1minJ = np.min(addTheta1)
#     #     minJ1index = np.argmin(addTheta1)
#     #     if(0 <= addTheta1minJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=minJ1index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     minusTheta4 = (theta[:,3] - 170)
#     #     minusTheta4maxJ = np.max(minusTheta4)
#     #     maxJ4index = np.argmax(minusTheta4)
#     #     if(0 > minusTheta4maxJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=maxJ4index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     addTheta4 = (theta[:,3] + 170)
#     #     addTheta4minJ = np.min(addTheta4)
#     #     minJ4index = np.argmin(addTheta4)
#     #     if(0 <= addTheta4minJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=minJ4index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass
    
#     # # select axis 3**************
#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     minusTheta3 = (theta[:,2] - 150)
#     #     minusTheta3maxJ = np.max(minusTheta3)
#     #     maxJ3index = np.argmax(minusTheta3)
#     #     if(0 > minusTheta3maxJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=maxJ3index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     addTheta3 = (theta[:,2] + 150)
#     #     addTheta3minJ = np.min(addTheta3)
#     #     minJ3index = np.argmin(addTheta3)
#     #     if(0 <= addTheta3minJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=minJ3index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # # select axis 6**************
#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     minusTheta6 = (theta[:,5] - 180)
#     #     minusTheta6maxJ = np.max(minusTheta6)
#     #     maxJ6index = np.argmax(minusTheta6)
#     #     if(0 > minusTheta6maxJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=maxJ6index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # while (1):
#     #     if(0>=numOfSolution):
#     #         break
#     #     addTheta6 = (theta[:,5] + 180)
#     #     addTheta6minJ = np.min(addTheta6)
#     #     minJ6index = np.argmin(addTheta6)
#     #     if(0 <= addTheta6minJ):
#     #         break
#     #     else:
#     #         theta = np.delete(theta,obj=minJ6index, axis=0) 
#     #         numOfSolution = numOfSolution-1
#     #     pass

#     # joint limit : axes 1  4  are [-170,170] 
#     #               axis2 is [-90,90]   axis5 is [-50,200]
#     #               axis3 is [-150,150] axis6 is [-180,180]

    
#     # optimize
#     # select suitable solution
#     wct = [4,12,6,2,2,1]

#     if(0 < numOfSolution): 
#         while(numOfSolution > 1):
#             minusTheta = abs(theta - cJdeg)
           
#             timesTheta = minusTheta * wct
          
#             sumTheta = np.sum(timesTheta,axis=1)
           
#             minJindex = np.argmin(sumTheta)
            
#             if abs(theta[minJindex,0])-170<=0 and abs(theta[minJindex,3])-170<=0 and abs(theta[minJindex,1])-90<=0 \
#             and abs(theta[minJindex,2])-150<=0 and theta[minJindex,4]-200<=0 and theta[minJindex,4]+50>=0 \
#             and abs(theta[minJindex,5])-180<=0 :
#                 bestR = theta[minJindex,:]
                
#                 break
#             else:
#                 theta = np.delete(theta,obj=minJindex, axis=0) 
#                 numOfSolution = numOfSolution-1
#             pass
#         if(1 == numOfSolution):
#             if abs(theta[0])-170<=0 and abs(theta[3])-170<=0 and abs(theta[1])-90<=0 \
#             and abs(theta[2])-150<=0 and theta[4]-200<=0 and theta[4]+50>=0 \
#             and abs(theta[5])-180<=0 :
#                 bestR = theta
#             else:
#                 return cJdeg  
#     else:
#         return cJdeg
#         pass
    
#     return bestR

#     pass

# def TFMeef(xyz_eulZYX):
#     # type:(np.ndarray)->np.ndarray
   
#     alfa = xyz_eulZYX[3]
#     beta = xyz_eulZYX[4]
#     gama = xyz_eulZYX[5]

#     # 注意：此处T是浅拷贝，会直接改变xyz_eulZYX的值
#     T = np.eye(4,dtype= np.float)
#     T[0:3,3] = xyz_eulZYX[0:3]

#     T[0,0] = math.cos(alfa) * math.cos(beta) 
#     T[0,1] = math.cos(alfa) * math.sin(beta) * math.sin(gama) - math.sin(alfa) * math.cos(gama) 
#     T[0,2] = math.cos(alfa) * math.sin(beta) * math.cos(gama) + math.sin(alfa) * math.sin(gama)
#     T[1,0] = math.sin(alfa) * math.cos(beta)
#     T[1,1] = math.sin(alfa) * math.sin(beta) * math.sin(gama) + math.cos(alfa) * math.cos(gama) 
#     T[1,2] = math.sin(alfa) * math.sin(beta) * math.cos(gama) - math.cos(alfa) * math.sin(gama)
#     T[2,0] = -math.sin(beta)
#     T[2,1] = math.cos(beta) * math.sin(gama)
#     T[2,2] = math.cos(beta) * math.cos(gama)
    
#     return T
#     pass


def writePToFile(polyPoseLists, cmd_rate, usetime, step):
    # type:(np.ndarray, float, float, int)->None
    row,col = polyPoseLists.shape
    dir = '/home/wheeltec/Desktop/ti5Poly_writePLists.txt'
    
    with open(dir, 'w') as f:
        head = {'name':'polyPoseLists', 'writeTime':rospy.Time.now().to_sec(), 
                 'cmdRate':cmd_rate, 'cmdTime':usetime, 'step':step}
        msg_head = json.dumps(head)
        f.write(msg_head)

        for i in range(0, row):
            w_msg = "\r\n{}, {}, {}, {}, {}, {}".format(polyPoseLists[i,0],
                                                        polyPoseLists[i,1],
                                                        polyPoseLists[i,2],
                                                        polyPoseLists[i,3],
                                                        polyPoseLists[i,4],
                                                        polyPoseLists[i,5])
            f.write(w_msg)
            pass 
        f.write("\r\n")
        print("write polyPoseLists to {}".format(dir))
        pass
    
    
    pass

def linerPoly(xyz_eulZYX, tList, step):
    # type:(np.ndarray, np.ndarray, int)->np.ndarray
    step = int(step)
    poly_rate = step/(tList[-1] - tList[0])
    
    poly_stepValue = (xyz_eulZYX[-1] - xyz_eulZYX[0])/step
   
    polyJdegLists = np.zeros((step+1,6), dtype=np.float)
    
    poly_poseLists = np.zeros((step+1,6), dtype=np.float)

    T = TFMeef(xyz_eulZYX[0])
    polyJdegLists[0] = (ti5_optIK(cJdeg=np.zeros((1,6),np.float),Targ=T))
    poly_poseLists[0] = xyz_eulZYX[0,:] 
    print('poly_poseLists[0]')
    print(poly_poseLists[0])

    for i in range(1,step+1):
        poly_poseLists[i] = xyz_eulZYX[0] + poly_stepValue*i
        T = TFMeef(poly_poseLists[i])
        polyJdegLists[i] = ti5_optIK(cJdeg=polyJdegLists[i-1],Targ=T)
        
        pass
    print('poly_poseLists[-1]')
    print(poly_poseLists[-1])

    writePToFile(poly_poseLists, poly_rate, tList[-1] - tList[0], step)

    return polyJdegLists
    pass

def cartesian_LP_ms(xyz_eulZYX, tList, cmdRate, cmdTime):
    # type:(np.ndarray, np.ndarray, int, float)-> np.ndarray
    """cartesian linear Poly. Use  cmdRate and stepValue move cmdTime(s) along the point specified in xyz_eulZYX
       
        input:
            xyz_eulZYX: cartesian pose/point list (xyz and eul_ZYX)
            tList: time corresponding to pose
        output:
            polyJdegLists: interpolated joint angle(degree)
            endPose: end posture
    """
    step = int(cmdRate*cmdTime)
    poly_rate = cmdRate
    endPose = xyz_eulZYX[-1]
    pointDist = xyz_eulZYX[-1] - xyz_eulZYX[0]
    ratio = cmdTime/abs(tList[-1]-tList[0])
    for i in range(0,2): # just poly x y
        endPose[i] =xyz_eulZYX[0][i] + pointDist[i]*ratio
        pass
    poly_stepValue = (endPose - xyz_eulZYX[0])/step
   
    polyJdegLists = np.zeros((step+1,6), dtype=np.float)
    
    poly_poseLists = np.zeros((step+1,6), dtype=np.float)

    T = TFMeef(xyz_eulZYX[0])
    polyJdegLists[0] = (ti5_optIK(cJdeg=np.zeros((1,6),np.float),Targ=T))
    poly_poseLists[0] = xyz_eulZYX[0,:] 
   
    print('poly_poseLists[0]',poly_poseLists[0])
    for i in range(1,step+1):
        poly_poseLists[i] = xyz_eulZYX[0] + poly_stepValue*i
        T = TFMeef(poly_poseLists[i])
        polyJdegLists[i] = ti5_optIK(cJdeg=polyJdegLists[i-1],Targ=T)
        
        pass
    print('endPose',endPose)

    writePToFile(poly_poseLists, poly_rate, cmdTime, step)

    return polyJdegLists
    pass

def pub_jLists(pub,jLists,cmd_rate, time):
    # # type:(Any,np.ndarray,int,float)->None

    # 如果需要自动运行令auto_run=1
    auto_run = 1 # 默认不运行
    
    jointTarget = ti5_polyJC_Lists()
    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 7
    jointTarget.interpolyReq = 0 # not need interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = cmd_rate
    # **************************************************************************

    print("*********+++++pub_jLists start+++*********")
    if auto_run==1:
        #jointxRadList is deg
        
        nJ = np.zeros((int(np.size(jLists)/6),7), np.float)
        nJ[:,6] = -600000
        nJ[-int((np.size(jLists)*2)/15):-1,6] = 0 # last 80% point will close gripper
        nJ[-1,6] = 0
        nJ[:,0:6] = jLists
        
        print('***')
        print(jLists.shape[0])
        for i in range(0,nJ.shape[0]):
            jointTarget.joint1RadList.append(nJ[i,0])
            jointTarget.joint2RadList.append(nJ[i,1])
            jointTarget.joint3RadList.append(nJ[i,2])
            jointTarget.joint4RadList.append(nJ[i,3])
            jointTarget.joint5RadList.append(nJ[i,4])
            jointTarget.joint6RadList.append(nJ[i,5])
            jointTarget.joint7RadList.append(nJ[i,6])
        # print('jointTarget:')
        # for i in range(0,len(jointTarget.joint1RadList)):
        #     print(jointTarget.joint1RadList[i],\
        #           jointTarget.joint2RadList[i],\
        #           jointTarget.joint3RadList[i],\
        #           jointTarget.joint4RadList[i],\
        #           jointTarget.joint5RadList[i],\
        #           jointTarget.joint6RadList[i],\
        #           jointTarget.joint7RadList[i])

        pub.publish(jointTarget)
        print("pub_jLists ok")
       
    pass

def testMotor_LinerPoly(sJDeg, eJDeg, pub,cmd_rate,use_time):
    # 如果需要自动运行令auto_run=1
    auto_run = 1 # 
    
    jointTarget = ti5_polyJC_Lists()
    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 6
    jointTarget.interpolyReq = 10 # iner linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = int(cmd_rate)
    # **************************************************************************
    # print("*********+++++LinerPoly start+++*********")
    
    # from pose 0 to pose 1
    if auto_run==1:
        #jointxRadList is deg
      
        nJ = [sJDeg[0],    sJDeg[1],  sJDeg[2],   sJDeg[3],    sJDeg[4],    sJDeg[5],  0.0,
              eJDeg[0],    eJDeg[1],  eJDeg[2],   eJDeg[3],    eJDeg[4],    eJDeg[5],  0.0]

           
        for i in range(0,len(nJ)/7):
            jointTarget.joint1RadList.append(nJ[0+i*7])
            jointTarget.joint2RadList.append(nJ[1+i*7])
            jointTarget.joint3RadList.append(nJ[2+i*7])
            jointTarget.joint4RadList.append(nJ[3+i*7])
            jointTarget.joint5RadList.append(nJ[4+i*7])
            jointTarget.joint6RadList.append(nJ[5+i*7])
            jointTarget.joint7RadList.append(nJ[6+i*7])

        jointTarget.timeLists = [0, use_time]
    
        pub.publish(jointTarget)
        print("LinerPoly pub ok")
   
    pass
 
def JLinerPoly(sJDeg, eJDeg, pub,cmd_rate,use_time):
    
    jointTarget = ti5_polyJC_Lists()
    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 6
    jointTarget.interpolyReq = 0 # external J space linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = int(cmd_rate)
    # **************************************************************************
    
    poly_step = int(use_time*cmd_rate)
    #jointxRadList is deg
    sJ = np.array(sJDeg, np.float)
    eJ = np.array(eJDeg, np.float)
    cnJ = np.array(eJ-sJ,np.float)
    stepValue = np.array(cnJ/poly_step,np.float)

    for i in range(1,poly_step+1):
        jointTarget.joint1RadList.append(sJDeg[0] + (i*stepValue[0]))
        jointTarget.joint2RadList.append(sJDeg[1] + (i*stepValue[1]))
        jointTarget.joint3RadList.append(sJDeg[2] + (i*stepValue[2]))
        jointTarget.joint4RadList.append(sJDeg[3] + (i*stepValue[3]))
        jointTarget.joint5RadList.append(sJDeg[4] + (i*stepValue[4]))
        jointTarget.joint6RadList.append(sJDeg[5] + (i*stepValue[5]))
   
    pub.publish(jointTarget)
    print("LinerPoly pub ok")
    
    pass

def writeJToFile(polyJdegLists, cmd_rate, usetime, step, dir = '/home/wheeltec/Desktop/ti5IK_writeJdegList.txt'):
    # type:(np.ndarray, float, float, int, str)->None
    row,col = polyJdegLists.shape
    # dir = '/home/wheeltec/Desktop/ti5IK_writeJdegList.txt'
    # try:
    with open(dir, 'w') as f:
        head = {'name':'JdegLists', 'writeTime':rospy.Time.now().to_sec(), 
                 'cmdRate':cmd_rate, 'cmdTime':usetime, 'step':step}
        msg_head = json.dumps(head)
        f.write(msg_head)
    
        for i in range(0, row):
            w_msg = "\r\n{}, {}, {}, {}, {}, {}".format(polyJdegLists[i,0],
                                                                                polyJdegLists[i,1],
                                                                                polyJdegLists[i,2],
                                                                                polyJdegLists[i,3],
                                                                                polyJdegLists[i,4],
                                                                                polyJdegLists[i,5])
            f.write(w_msg)
            pass
        f.write("\r\n")
        print("write polyJdegLists to {}".format(dir))
        pass
    # except Exception as e:
    #     print('write polyJdegLists to file failed')
    #     print(e)


    pass

def sub_motorStat():
    
    """堵塞20ms至收到关节消息/deg,返回的关节角为二维数组
    """
    timeout = rospy.Duration(0,200000000) # 200ms
    try:
        motorStat = rospy.wait_for_message("ti5_motor_status", ti5_motorStatus, timeout=timeout) # type:ti5_motorStatus
    except Exception as e:
        print(e)
        return -1
        

    try:
        timeStamp = motorStat.header.stamp
        numOfJ = motorStat.numOfMotor

        jLists = np.array(motorStat.joint1_7DegLists, dtype=np.float) # type:np.ndarray

        step = int(jLists.size/numOfJ)
        jLists.resize((step,numOfJ))
        if jLists.size >0:
            # print('jLists')
            # print(jLists)
            # print(jLists.shape)
            
            return jLists
            pass
        else:
            print('jLists.size',jLists.size)
            return -1
        pass
    except Exception as e:
        print('can not sub msg: "ti5_motor_status" ')
        return -1
    
    pass

def get_msg_pubCMD(connect, pub):
    
    z = 30 # safety distance limit while gripper fully closed(/mm)
    grabTime = 0.5 # /s
    pose_flag = 0 # not ready
    cmd_flag = 0 # motor stoped
    tE = 0.5 # time error

    # ********24/2/25 modified********
    X_err = 0 # 坐标轴修正误差，由标定误差决定(/mm)
    Y_err = 0
    Z_err = 0
    # *******************************
    arm_cmdRate = 500

    pose_flag = 0
    motorStat = sub_motorStat() # get motor sta

    imge = np.zeros((480,460,3), dtype=np.uint8)#要显示的UI图像
    cv.imshow('ti5GrabFruits', imge)
    while True:
        key = cv.waitKey(2)
        if key & 0xff == ord('q'):
            cv.destroyAllWindows()
            break

        if cmd_flag == 1 or pose_flag < 2 : # motor running or pose not ready
        
            motorStat = sub_motorStat() # get motor sta
            pass
        

        recv = connect.recv(1000).decode() # get msg

        if cmd_flag==1 and type(-1)==type(motorStat): # motor running and get nothing
            # pose_flag = 0 # not ready

            connect.send('motor running'.encode())
            print('motor running')
            
            continue
            pass
        if  pose_flag < 2 and type(-1)!=type(motorStat): # pose not ready and get motor sta
            if (np.sum(abs(motorStat[0] - np.array([0,0,-90,0,0,0,-32.6], np.float)))>0.7): # motors are not ready sta
                pose_flag = 0 # not ready
                print('current state is not ready')
                connect.send('current state is not ready'.encode())
                print('dist between ready pose and curr Pose',np.sum(abs(motorStat[0] - np.array([0,0,-90,0,0,0,-32.6], np.float))))
                print(motorStat[0])
                continue
            else: # mortors are at ready pose
                pose_flag = pose_flag+1 # pose ready
                if pose_flag>=2:
                    cmd_flag = 0 # motor stoped
                    pass
                pass
                print('dist between ready pose and curr Pose',np.sum(abs(motorStat[0] - np.array([0,0,-90,0,0,0,-32.6], np.float))))
                print(motorStat[0])

            motorStat = -1
            pass
        
        
        if recv:
            if recv=="ok":
                # print("ooo")
                connect.send('ooo'.encode())
                continue
            else: # recved msg
                print('recv')
                print(recv)
            if pose_flag<2: # pose not ready
                print('receive a msg, but current state is not ready, msg was ignored!')
                connect.send('current state is not ready, previous msg was ignored!'.encode())
                continue
                
            data = json.loads(recv)
            print('data')
            print(type(data))
            print(data)
            x1 = data['X1']*1000 + X_err # 由于相机标定误差
            x2 = data['X2']*1000 + X_err # 位置参数需要修正
            y1 = data['Y1']*1000 + Y_err
            y2 = data['Y2']*1000 + Y_err
            z1 = data['Z1']*1000 + Z_err
            z2 = data['Z2']*1000 + Z_err
            t1 = data['T1']
            t2 = data['T2']

            T1 = TFMeef(np.array([x1,y1,z,90*pi/180,0,180*pi/180],np.float))
            print('TfM1')
            print(T1)

            jdeg1 = ti5_optIK(cJdeg=np.array([0,0,-90,0,0,0],np.float),Targ=T1)
            print('j1')
            print(np.around(jdeg1,3))

            T2 = TFMeef(np.array([x2,y2,z,90*pi/180,0,180*pi/180],np.float))
            print('TfM2')
            print(T2)

            jdeg2 = ti5_optIK(cJdeg=np.array(jdeg1,np.float),Targ=T2)
            print('j2')
            print(np.around(jdeg2,3))


            distance = sum(abs(jdeg2 - jdeg1))
            if 0.5<distance: # msg ok
                print(rospy.Time.now().to_nsec())
                time1=rospy.Time.now().to_nsec()
                openGripper(pub, -600000.0) # open gripper
                testMotor_LinerPoly([0,0,-90,0,0,0] ,jdeg1, pub, 500, t1-tE) # move to point1
                print('move to point1')

                cmd_rate = arm_cmdRate

                # # follow more
                # polyJdegLists = linerPoly(np.array([[x1,y1,z,90*pi/180,0,180*pi/180],
                #                                     [x2,y2,z,90*pi/180,0,180*pi/180]],dtype=np.float), 
                #                           np.array([0,t2],dtype=np.float), 
                #                           cmd_rate*t2)

                # follow less
                polyJdegLists = cartesian_LP_ms(np.array([[x1,y1,z,90*pi/180,0,180*pi/180],
                                                          [x2,y2,z,90*pi/180,0,180*pi/180]],dtype=np.float), 
                                                np.array([0,t2],dtype=np.float), 
                                                cmd_rate,
                                                grabTime)

                print('polyJdegLists.shape', polyJdegLists.shape)
                
                # # follow more
                # pub_jLists(pub, polyJdegLists,cmd_rate, t2)
                
                # follow less
                pub_jLists(pub, polyJdegLists,cmd_rate, grabTime) # move toward point2 and grab


                TGup = ti5FK(polyJdegLists[-1]*(pi/180))
                TGup[2,3] = 200
                print('TGup')
                print(TGup)

                jdegGup = ti5_optIK(cJdeg=polyJdegLists[-1],Targ=TGup)
                print('jdegGup')
                print(np.around(jdegGup,3))

                TPutUp = TFMeef(np.array([0,-450,200,90*pi/180,0,180*pi/180],np.float))
                print('TPutUp')
                print(TPutUp)

                jdegPutUp = ti5_optIK(cJdeg=np.array(jdegGup,np.float),Targ=TPutUp)
                print('jdegPutUp')
                print(np.around(jdegPutUp,3))

                TPutDown = TFMeef(np.array([0,-450,-200,90*pi/180,0,180*pi/180],np.float))
                print('TPutDown')
                print(TPutDown)

                jdegPutDown = ti5_optIK(cJdeg=np.array(jdegPutUp,np.float),Targ=TPutDown)
                print('jdegPutDown')
                print(np.around(jdegPutDown,3))
                
              
                # # follow less
                # testMotor_LinerPoly(polyJdegLists[-1], [0,0,-90,0,0,0], pub, 500, 6.0) # back to ready pose

                # move to grab up pose
                testMotor_LinerPoly(polyJdegLists[-1], jdegGup, pub, 500, 1.0)

                # move to put up pose
                testMotor_LinerPoly(jdegGup, jdegPutUp, pub, 500, 3.0)
               
                # move to put down pose
                testMotor_LinerPoly(jdegPutUp, jdegPutDown, pub, 500, 3.0)

                openGripper(pub, -600000.0) # open gripper
                
                # move to put up pose
                jdegPutUp[5]=0.0
                testMotor_LinerPoly(jdegPutDown, jdegPutUp, pub, 500, 3.0)
               
                # move to ready pose
                testMotor_LinerPoly(jdegPutUp, [0,0,-90,0,0,0], pub, 500, 2.0)
               

                print(rospy.Time.now().to_nsec())
                time2 = rospy.Time.now().to_nsec()
                print((time2-time1)/1000000)

                # write polyJdegLists to file
                # dir = '/home/wheeltec/Desktop/ti5put_JdegList.txt'
                writeJToFile(polyJdegLists, cmd_rate, grabTime, cmd_rate*grabTime)

                connect.send('ok'.encode())
                cmd_flag = 1 # motor running
                pose_flag = 0 # pose not ready
            else:
                print('error data')
                connect.send('error data'.encode())
            pass # end if recv
        else:
            print('error data')
            pass

    pass

def sockLink(localhost, local_port, pub):
    s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    print('connet to client...')
    s.bind((localhost, local_port))
    s.listen(1)
    print('server is listening...')
    client_sock, client_addr = s.accept()
    print('addr',client_addr)
    print('connet successfully!')
    get_msg_pubCMD(client_sock, pub)
    client_sock.close()
    print('connet closed!')
    pass

def openGripper(pub, pose):
    jointTarget = ti5_polyJC_Lists()
    # **************************config ctrl mode********************************
    jointTarget.numOfCmd = 1
    jointTarget.interpolyReq = 10 # iner linear interpoly
    jointTarget.mode = 0
    jointTarget.header.stamp = rospy.Time().now()
    jointTarget.cmdRate = int(500)
    # **************************************************************************
    print("*********+++++openGripper start+++*********")
    nJ = [  0.0,    0.0,   -90,     0.0,    0.0,   0.0,  float(pose)]
    for i in range(0,len(nJ)/7):
        jointTarget.joint1RadList.append(nJ[0+i*7])
        jointTarget.joint2RadList.append(nJ[1+i*7])
        jointTarget.joint3RadList.append(nJ[2+i*7])
        jointTarget.joint4RadList.append(nJ[3+i*7])
        jointTarget.joint5RadList.append(nJ[4+i*7])
        jointTarget.joint6RadList.append(nJ[5+i*7])
        jointTarget.joint7RadList.append(nJ[6+i*7])

        jointTarget.timeLists = [0, 4]
    
        pub.publish(jointTarget)
        print("openGripper pub ok")


    pass

def testPolyResult(dirJ, dirP, line):
    
    # read JdegLists**********************
    with open(dirJ, mode='r') as Jfile:
        Jdata = Jfile.readlines()
        pass
    groups_jdeg = (len(Jdata)-1)
    try:
        head = json.loads(Jdata[0])
        name = head['name']
        cmdTime = head['cmdTime']
        cmdRate = head['cmdRate']
        step = head['step']
    except Exception as e:
        print('json loads msg_head failed!')
        print(e)
        name = ''

    if(name=="JdegLists"):# 
        print("open file!")
        print("groups of Jdeg:",groups_jdeg)
        Jdata_split = Jdata[line-1].split(', ')
        Jdeg = np.zeros(6, dtype=np.float)
        for i in range(0,6):
            Jdeg[i] = float(Jdata_split[i])
        print("line={}\r\nreadJdeg=\r\n{}".format(line,Jdeg))
    
    # read polyPoseLists**********************
    with open(dirP, mode='r') as Pfile:
        Pdata = Pfile.readlines()
        pass
    groups_P = (len(Pdata)-1)

    try:
        head = json.loads(Pdata[0])
        name = head['name']
        cmdTime = head['cmdTime']
        cmdRate = head['cmdRate']
        step = head['step']
    except Exception as e:
        print('json loads msg_head failed!')
        print(e)
        name = ''
    if(name=="polyPoseLists"):# 
        print("open file!")
        print("groups of Pose:",groups_P)
        Pdata_split = Pdata[line-1].split(', ')
        Pose = np.zeros(6, dtype=np.float)
        for i in range(0,6):
            Pose[i] = float(Pdata_split[i])
     
        print("line={}\r\nreadPose=\r\n{}".format(line,Pose))

    

    T_poly_pose = TFMeef(Pose)
    print("TFMeef M by read Pose:\r\n{}".format(T_poly_pose))

    IK_deg = ti5_optIK(cJdeg=np.zeros((1,6),np.float),Targ=T_poly_pose)
    print("IK deg by read Pose:\r\n{}".format(IK_deg))

    FKxyz=ti5FK(Jdeg*(pi/180))
    print("FK M by read Jdeg:\r\n{}".format(FKxyz))

    FKik=ti5FK(IK_deg*(pi/180))
    print("FK M by IK_deg:\r\n{}".format(FKik))
  

    pass



if  __name__== "__main__":
 
    rospy.init_node("ti5GrabFruits_node")

    #**********连接视觉，开始抓取***********
    pub = rospy.Publisher("interPolyPoint_topic",ti5_polyJC_Lists,queue_size=1000)

    rate = rospy.Rate(1)
    rate.sleep()
    rate.sleep()
    rate.sleep()#waiter for system

    localhost = '0.0.0.0'
    local_port = 38899

    openGripper(pub, -600000.0)
    sockLink(localhost, local_port, pub)
    #************************************
 

    pass

 





 


  

   

